#include "ImageLandmarksLikelyhoodEvaluator.h"

using namespace mrpt::slam;

CImageLandmarksLikelihoodEvaluator::CImageLandmarksLikelihoodEvaluator(void)
{
	stdDev  = 1;
	stdDev2 = 1;
	MIN_LIKELIHOOD = -1000;
}

CImageLandmarksLikelihoodEvaluator::~CImageLandmarksLikelihoodEvaluator(void)
{
}

double CImageLandmarksLikelihoodEvaluator::eval(const mrpt::slam::CObservation* pObs1, const mrpt::slam::CObservation* pObs2) const {

	const CObservationImageLandmarks* pLandmarks1 = (CObservationImageLandmarks*)pObs1;
	const CObservationImageLandmarks* pLandmarks2 = (CObservationImageLandmarks*)pObs2;

	int size = pLandmarks1->sensedData.size();

	if (size == 0) return MIN_LIKELIHOOD;

	double sum = 0;

	for (size_t i = 0; i < size; i++) {
		
		int id = pLandmarks1->sensedData[i].id;
		double x1 = pLandmarks1->sensedData[i].x;

		bool found = false;
		//iterate landmarks2, find same id landamark
		for (size_t j = 0; j < pLandmarks2->sensedData.size(); j++) {
			int id2 = pLandmarks2->sensedData[j].id;
			double x2 = pLandmarks2->sensedData[j].x;
			if (id == id2) {
				double likely = -(x1 - x2) * (x1 - x2) / stdDev2;
				if (likely < MIN_LIKELIHOOD) {
					likely = MIN_LIKELIHOOD;
				}
				sum += likely;
				found = true;
			}
		}

		if (!found) {
			sum += MIN_LIKELIHOOD;
		}
	}

	return sum / size;
}